#include "controllerPerseguir.h"


void ControllerPerseguir::setTarget(int _targetId, int _targetLayer){
	Scene * scene = Scene::getScene();
	targetId = _targetId;
	targetLayer = _targetLayer;
	targetPos = *scene->getInstance(targetId,targetLayer)->model * Vector3(0,0,0);
}

void ControllerPerseguir::recalc(int _object, int _layer, double _seconds_elapsed){
	
	//objecto a seguir
	if(targetId == 0) targetId = 0;
	if(targetLayer == 0) targetLayer = 1;

	Scene * scene = Scene::getScene();
	Vector3 myPos = *scene->getInstance(_object,_layer)->model * Vector3(0,0,0);
	targetOldPos = targetPos;
	Vector3 targetPos = *scene->getInstance(targetId,targetLayer)->model * Vector3(0,0,0);
	targetSpeed = targetPos - targetOldPos;
	float distance = getDistance(myPos,targetPos);
	//objecto a mover
	Ship * ship = (Ship*) scene->getInstance(_object, _layer);
	
	if(ship->alive && abs(distance) < 1500.0 )
	{
		Matrix44 * m = scene->getInstance(_object,_layer)->model;
		Vector3 f = m->frontVector();
		Vector3 r = m->rightVector();
		Vector3 u = m->topVector();
		Vector3 p = *m * Vector3(0,0,0);

		float vel_ang = ((float)_seconds_elapsed * 1.0f);
		
		//PLANO IZQUIERDA/DERECHA
		float A = r.x;
		float B = r.y;
		float C = r.z;
		float D = -(r.x*p.x + r.y*p.y + r.z*p.z) ;
		float resLR, resUD;

		resLR = A*targetPos.x +B*targetPos.y +C*targetPos.z + D;
		

		int mov = 1;
		if (getDistance(myPos,targetPos) < 10) 
			mov = -1;
		bool control = true;
		
		
		//PLANO ARRIBA/ABAJO
		A = u.x;
		B = u.y;
		C = u.z;
		D =-( u.x*p.x + u.y*p.y + u.z*p.z) ;
		
		resUD = A*targetPos.x + B*targetPos.y + C*targetPos.z + D;
		
		if (resLR > 0){
			//doblar derecha
				if (control) ship->Move("right",80*mov, _seconds_elapsed);
				if (!control) {
					m->rotateLocal(vel_ang,Vector3(0,mov,0));
				}
		}
		else if (resLR < 0) {
			//doblar izquierda
				if (control) ship->Move("left",80*mov, _seconds_elapsed);
				if (!control) m->rotateLocal(vel_ang,Vector3(0,-mov,0));
		}

		if (resUD > 0){
			//SUBIR
			if (control) ship->Move("up",80*mov, _seconds_elapsed);
			if (!control) m->rotateLocal(vel_ang,Vector3(-mov,0,0));
		}
		else if (resUD < 0) {
			//bajar
			if (control) ship->Move("down",80*mov, _seconds_elapsed);
			if (!control) m->rotateLocal(vel_ang,Vector3(mov,0,0));
		}
		//derecha arriba/abajo
		if(resLR > 0 && resUD != 0) ship->Move("roll_right",50*mov, _seconds_elapsed);
		//izquierda arriba/abajo
		if(resLR < 0 && resUD != 0) ship->Move("roll_left",50*mov, _seconds_elapsed);
		
		
		if (control){
			ship->Move("forward",80, _seconds_elapsed);
		}else {
			m->traslateLocal(0.0f,0.0f,-(float)_seconds_elapsed * 100.0f);
		}
		
		//Comprobamos si tiene que disparar
		//Vector3 FireLine = getFireLine(_object, targetPos, targetSpeed);
		Vector3 FireLine = getFireLine(_object, targetPos);

		if( isOnFireLine(_object, FireLine, 0.99999) ) {
			
			fireTarget(_object, FireLine, 3);
			fireTarget(_object, FireLine, 1);
		}
		else if( isOnFireLine(_object, FireLine, 0.9999) ) {
			
			fireTarget(_object, FireLine, 2);
			fireTarget(_object, FireLine, 1);
		}
		else if( isOnFireLine(_object, FireLine, 0.99) ) {
			
			fireTarget(_object, FireLine, 1);
		}
	}
}
